#include "maindefs.h"
#ifndef __XC8
#include <i2c.h>
#else
#include <plib/i2c.h>
#endif
#include "my_i2c.h"
#include "my_leds.h"

// =============================================================================
// = SSPSTAT register constants
// =============================================================================

/*
 * SSPSTAT
 *
 * The following are bits that should be bitwise or'd
 * to set certain conditions for I2C.
 *
 * SLEW: Enable for high-speed (400kHz) or disable for normal-speed (100kHz or 1MHz)
 * SMBUS: Enable or disable (disabled with Jones slave I2C)
 */
#define I2C_ENABLE_SLEW     0b00000000 // Sets SMP to 0, which is enabled
#define I2C_DISABLE_SLEW    0b10000000 // Sets SMP to 1, which is disabled
#define I2C_ENABLE_SMBUS    0b01000000 // Sets CKE to 1, which is enabled
#define I2C_DISABLE_SMBUS   0b00000000 // Sets CKE to 0, which is disabled

#define I2C_RECEIVED_BUFFER_FULL \
                            0b00000001 // If 1, indicates that data has been
                                       // received and is in the SSPBUF register
#define I2C_SENDING_BUFFER_FULL \
                            0b00000001 // If 1, indicates that data is still
                                       // being sent by the master

// =============================================================================
// = SSPCON1 register constants
// =============================================================================

/*
 * SSPCON1
 *
 * The following are bits that should be bitwise or'd
 * to set certain conditions for I2C.
 */
#define I2C_MASTER_MODE     0b00001000
#define I2C_ENABLE          0b00100000 // Sets SSPEN to 1, which is enabled

// =============================================================================
// = SSPCON2 register constants
// =============================================================================

/*
 * SSPCON2
 *
 * The following are bits that should be bitwise or'd
 * to set certain conditions for I2C.
 */

// =============================================================================
// = SSPADD register constants
// =============================================================================

/*
 * The following is the proper value for the SSPADD register
 * to provide a 100kHz I2C bus for master given the following:
 * 1. The microcontroller is operating at 32MHz.
 *
 * Math: Fosc / (4 * (SSPADD + 1))
 * (32000000 / (4 * (79+1))) == 100000 (100kHz)
 */
#define I2C_100KHZ 0x28  //79

// =============================================================================
// = Clock and data bits
// = 0: Output
// = 1: Input
// =============================================================================

#ifdef __USE18F26J50
#define I2C_SCL PORTBbits.SCL1
#define I2C_SDA PORTBbits.SDA1
#else
#define I2C_SCL TRISCbits.TRISC3
#define I2C_SDA TRISCbits.TRISC4
#endif

static i2c_comm *ic_ptr;

// =============================================================================
// = Public functions
// =============================================================================

// Configure for I2C Master mode -- the variable "slave_addr" should be stored in
//   i2c_comm (as pointed to by ic_ptr) for later use.

void i2c_configure_master(unsigned char slave_addr) {
    ic_ptr->slave_addr = slave_addr;
    ic_ptr->status = I2C_MASTER_IDLE;

    // Set all to zero so can or bits later
    SSPSTAT = 0x00;
    SSPCON1 = 0x00;
    SSPCON2 = 0x00;

    // Disable slew for a 100kHz bus speed
    // Disable SMBus
    SSPSTAT |= I2C_DISABLE_SLEW | I2C_DISABLE_SMBUS;

    // Set I2C to master mode
    SSPCON1 |= I2C_MASTER_MODE;

    // Set I2C bus to operate at 100kHz
    SSPADD = I2C_100KHZ;

    // Enable I2C
    SSPCON1 |= I2C_ENABLE;

    // Set clock and data pins for output
    I2C_SCL = 1;
    I2C_SDA = 1;
}

void i2c_master_set_target(unsigned char slave_addr) {
    ic_ptr->slave_addr = slave_addr;
}

// Sending in I2C Master mode [slave write]
// 		returns -1 if the i2c bus is busy
// 		return 0 otherwise
// Will start the sending of an i2c message -- interrupt handler will take care of
//   completing the message send.  When the i2c message is sent (or the send has failed)
//   the interrupt handler will send an internal_message of type MSGT_MASTER_SEND_COMPLETE if
//   the send was successful and an internal_message of type MSGT_MASTER_SEND_FAILED if the
//   send failed (e.g., if the slave did not acknowledge).  Both of these internal_messages
//   will have a length of 0.
// The subroutine must copy the msg to be sent from the "msg" parameter below into
//   the structure to which ic_ptr points [there is already a suitable buffer there].

unsigned char i2c_master_send(unsigned char length, unsigned char *msg) {
    // TODO: Add check to return -1
    //if (( SSPCON2 & 0x1F ) || ( SSPSTATbits.R_W ))
    if (ic_ptr->status != I2C_MASTER_IDLE)
        return -1;

    for (ic_ptr->outbuflen = 0; ic_ptr->outbuflen < length; ic_ptr->outbuflen++) {
        ic_ptr->outbuffer[ic_ptr->outbuflen] = msg[ic_ptr->outbuflen];
    }
    ic_ptr->outbuflen = length;
    ic_ptr->outbufind = 0;

    // Update status to handle master sending data
    ic_ptr->status = I2C_MASTER_START_SEND;

    StartI2C();
    return(0);
}

// Receiving in I2C Master mode [slave read]
// 		returns -1 if the i2c bus is busy
// 		return 0 otherwise
// Will start the receiving of an i2c message -- interrupt handler will take care of
//   completing the i2c message receive.  When the receive is complete (or has failed)
//   the interrupt handler will send an internal_message of type MSGT_MASTER_RECV_COMPLETE if
//   the receive was successful and an internal_message of type MSGT_MASTER_RECV_FAILED if the
//   receive failed (e.g., if the slave did not acknowledge).  In the failure case
//   the internal_message will be of length 0.  In the successful case, the
//   internal_message will contain the message that was received [where the length
//   is determined by the parameter passed to i2c_master_recv()].
// The interrupt handler will be responsible for copying the message received into

unsigned char i2c_master_recv(unsigned char length) {
    // TODO: Add check to return -1
    //if (( SSPCON2 & 0x1F ) || ( SSPSTATbits.R_W ))
    if (ic_ptr->status != I2C_MASTER_IDLE)
        return -1;

    ic_ptr->bufind = 0;
    ic_ptr->buflen = length;

    // Update status to handle master sending data
    ic_ptr->status = I2C_MASTER_START_RECEIVE;

    StartI2C();
    return(0);
}

i2c_comm* i2c_communication(void) {
    return ic_ptr;
}

void start_i2c_slave_reply(unsigned char length, unsigned char *msg) {

    for (ic_ptr->outbuflen = 0; ic_ptr->outbuflen < length; ic_ptr->outbuflen++) {
        ic_ptr->outbuffer[ic_ptr->outbuflen] = msg[ic_ptr->outbuflen];
    }
    ic_ptr->outbuflen = length;
    ic_ptr->outbufind = 1; // point to the second byte to be sent

    // put the first byte into the I2C peripheral
    SSPBUF = ic_ptr->outbuffer[0];
    // we must be ready to go at this point, because we'll be releasing the I2C
    // peripheral which will soon trigger an interrupt
    SSPCON1bits.CKP = 1;

}

// an internal subroutine used in the slave version of the i2c_int_handler

void handle_start(unsigned char data_read) {
    ic_ptr->event_count = 1;
    ic_ptr->buflen = 0;
    // check to see if we also got the address
    if (data_read) {
        if (SSPSTATbits.D_A == 1) {
            // this is bad because we got data and
            // we wanted an address
            ic_ptr->status = I2C_IDLE;
            ic_ptr->error_count++;
            ic_ptr->error_code = I2C_ERR_NOADDR;
        } else {
            if (SSPSTATbits.R_W == 1) {
                ic_ptr->status = I2C_SLAVE_SEND;
            } else {
                ic_ptr->status = I2C_RCV_DATA;
            }
        }
    } else {
        ic_ptr->status = I2C_STARTED;
    }
}

// this is the interrupt handler for i2c -- it is currently built for slave mode
// -- to add master mode, you should determine (at the top of the interrupt handler)
//    which mode you are in and call the appropriate subroutine.  The existing code
//    below should be moved into its own "i2c_slave_handler()" routine and the new
//    master code should be in a subroutine called "i2c_master_handler()"

void i2c_int_handler() {
    unsigned char i2c_data;
    unsigned char data_read = 0;
    unsigned char data_written = 0;
    unsigned char master_data_read = 0;
    unsigned char msg_ready = 0;
    unsigned char msg_to_send = 0;
    unsigned char overrun_error = 0;
    unsigned char error_buf[3];

    // clear SSPOV
    if (SSPCON1bits.SSPOV == 1) {
        SSPCON1bits.SSPOV = 0;
        // we failed to read the buffer in time, so we know we
        // can't properly receive this message, just put us in the
        // a state where we are looking for a new message
        ic_ptr->status = I2C_IDLE;
        overrun_error = 1;
        ic_ptr->error_count++;
        ic_ptr->error_code = I2C_ERR_OVERRUN;
    }
    // read something if it is there
    if (SSPSTATbits.BF == 1) {
        i2c_data = SSPBUF;
        data_read = 1;
    }

    if (!overrun_error) {
        switch (ic_ptr->status) {
#if !I2C_MASTER
            case I2C_IDLE:
            {
                // ignore anything except a start
                if (SSPSTATbits.S == 1) {
                    handle_start(data_read);
                    // if we see a slave read, then we need to handle it here
                    if (ic_ptr->status == I2C_SLAVE_SEND) {
                        data_read = 0;
                        msg_to_send = 1;
                    }
                }
                break;
            }
            case I2C_STARTED:
            {
                // in this case, we expect either an address or a stop bit
                if (SSPSTATbits.P == 1) {
                    // we need to check to see if we also read an
                    // address (a message of length 0)
                    ic_ptr->event_count++;
                    if (data_read) {
                        if (SSPSTATbits.D_A == 0) {
                            msg_ready = 1;
                        } else {
                            ic_ptr->error_count++;
                            ic_ptr->error_code = I2C_ERR_NODATA;
                        }
                    }
                    ic_ptr->status = I2C_IDLE;
                } else if (data_read) {
                    ic_ptr->event_count++;
                    if (SSPSTATbits.D_A == 0) {
                        if (SSPSTATbits.R_W == 0) { // slave write
                            ic_ptr->status = I2C_RCV_DATA;
                        } else { // slave read
                            ic_ptr->status = I2C_SLAVE_SEND;
                            msg_to_send = 1;
                            // don't let the clock stretching bit be let go
                            data_read = 0;
                        }
                    } else {
                        ic_ptr->error_count++;
                        ic_ptr->status = I2C_IDLE;
                        ic_ptr->error_code = I2C_ERR_NODATA;
                    }
                }
                break;
            }
            case I2C_SLAVE_SEND:
            {
                if (ic_ptr->outbufind < ic_ptr->outbuflen) {
                    SSPBUF = ic_ptr->outbuffer[ic_ptr->outbufind];
                    ic_ptr->outbufind++;
                    data_written = 1;
                } else {
                    // we have nothing left to send
                    ic_ptr->status = I2C_IDLE;
                }
                break;
            }
            case I2C_RCV_DATA:
            {
                // we expect either data or a stop bit or a (if a restart, an addr)
                if (SSPSTATbits.P == 1) {
                    // we need to check to see if we also read data
                    ic_ptr->event_count++;
                    if (data_read) {
                        if (SSPSTATbits.D_A == 1) {
                            ic_ptr->buffer[ic_ptr->buflen] = i2c_data;
                            ic_ptr->buflen++;
                            msg_ready = 1;
                        } else {
                            ic_ptr->error_count++;
                            ic_ptr->error_code = I2C_ERR_NODATA;
                            ic_ptr->status = I2C_IDLE;
                        }
                    } else {
                        msg_ready = 1;
                    }
                    ic_ptr->status = I2C_IDLE;
                } else if (data_read) {
                    ic_ptr->event_count++;
                    if (SSPSTATbits.D_A == 1) {
                        ic_ptr->buffer[ic_ptr->buflen] = i2c_data;
                        ic_ptr->buflen++;
                    } else /* a restart */ {
                        if (SSPSTATbits.R_W == 1) {
                            ic_ptr->status = I2C_SLAVE_SEND;
                            msg_ready = 1;
                            msg_to_send = 1;
                            // don't let the clock stretching bit be let go
                            data_read = 0;
                        } else { /* bad to recv an address again, we aren't ready */
                            ic_ptr->error_count++;
                            ic_ptr->error_code = I2C_ERR_NODATA;
                            ic_ptr->status = I2C_IDLE;
                        }
                    }
                }
                break;
            }
#else
            case I2C_MASTER_IDLE:
            {
                break;
            }
            case I2C_MASTER_START_SEND:
            {
                ic_ptr->status = I2C_MASTER_SEND;
                
                // Read to ensure buffer is clear
                {unsigned char tmp = SSPBUF;}

                SSPBUF = (ic_ptr->slave_addr | 0x00);
                DBG1_TOGGLE();
                break;
            }
            case I2C_MASTER_START_RECEIVE:
            {
                ic_ptr->status = I2C_MASTER_RECEIVE_SEND_ADDRESS;

                // Read to ensure buffer is clear
                {unsigned char tmp = SSPBUF;}

                SSPBUF = (ic_ptr->slave_addr | 0x01);
                DBG0_TOGGLE();
                break;
            }
            case I2C_MASTER_RESTART:
            {
                break;
            }
            case I2C_MASTER_STOP:
            {
                ic_ptr->status = I2C_MASTER_IDLE;
                break;
            }
            case I2C_MASTER_SEND_ACK:
            {
                ic_ptr->status = I2C_MASTER_SEND;
                break;
            }
            case I2C_MASTER_RECEIVE_ACK:
            {
                ic_ptr->status = I2C_MASTER_RECEIVE;
                SSPCON2bits.RCEN = 1;
                break;
            }
            case I2C_MASTER_NACK:
            {
                ic_ptr->status = I2C_MASTER_STOP;
                StopI2C();
                break;
            }
            case I2C_MASTER_SEND:
            {
                if (ic_ptr->outbufind < ic_ptr->outbuflen) {
                    SSPBUF = ic_ptr->outbuffer[ic_ptr->outbufind++];
                } else {
                    // we have nothing left to send
                    ic_ptr->status = I2C_MASTER_STOP;
                    StopI2C();
                }
                break;
            }
            case I2C_MASTER_RECEIVE_SEND_ADDRESS:
            {
                ic_ptr->status = I2C_MASTER_RECEIVE;
                SSPCON2bits.RCEN = 1;
                break;
            }
            case I2C_MASTER_RECEIVE:
            {
                ic_ptr->buffer[ic_ptr->bufind++] = SSPBUF;

                if (ic_ptr->bufind < ic_ptr->buflen) {
                    ic_ptr->status = I2C_MASTER_RECEIVE_ACK;
                    AckI2C();
                } else {
                    // we have nothing left to read
                    master_data_read = 1; // Enable to send out via UART
                    ic_ptr->status = I2C_MASTER_NACK;
                    NotAckI2C();
                }
                break;
            }

            default:
            {
                break;
            }
#endif
        }
    }

    // release the clock stretching bit (if we should)
    if (data_read || data_written) {
        // release the clock
        if (SSPCON1bits.CKP == 0) {
            SSPCON1bits.CKP = 1;
        }
    }

    // must check if the message is too long, if
    if ((ic_ptr->buflen > MAXI2CBUF - 2) && (!msg_ready)) {
        ic_ptr->status = I2C_IDLE;
        ic_ptr->error_count++;
        ic_ptr->error_code = I2C_ERR_MSGTOOLONG;
    }

    if (msg_ready) {
        ic_ptr->buffer[ic_ptr->buflen] = ic_ptr->event_count;
        ToMainHigh_sendmsg(ic_ptr->buflen + 1, MSGT_I2C_DATA, (void *) ic_ptr->buffer);
        ic_ptr->buflen = 0;
    } else if (ic_ptr->error_count >= I2C_ERR_THRESHOLD) {
        error_buf[0] = ic_ptr->error_count;
        error_buf[1] = ic_ptr->error_code;
        error_buf[2] = ic_ptr->event_count;
        ToMainHigh_sendmsg(sizeof (unsigned char) *3, MSGT_I2C_DBG, (void *) error_buf);
        ic_ptr->error_count = 0;
    }
    if (msg_to_send) {
        // send to the queue to *ask* for the data to be sent out
        ToMainHigh_sendmsg(0, MSGT_I2C_RQST, (void *) ic_ptr->buffer);
        msg_to_send = 0;
    }
    
#if I2C_MASTER // Master I2C
    // I2C finished receiving data
    if (master_data_read) {
        ToMainHigh_sendmsg(ic_ptr->buflen, MSGT_I2C_TO_UART, (void *) ic_ptr->buffer);
        //uart_send(ic_ptr->buflen, (void *) ic_ptr->buffer);
        ic_ptr->buflen = 0;
        master_data_read = 0;
    }
#endif
}

// set up the data structures for this i2c code
// should be called once before any i2c routines are called

void init_i2c(i2c_comm *ic) {
    ic_ptr = ic;
    ic_ptr->buflen = 0;
    ic_ptr->event_count = 0;
    ic_ptr->status = I2C_IDLE;
    ic_ptr->error_count = 0;
}

// setup the PIC to operate as a slave
// the address must include the R/W bit

void i2c_configure_slave(unsigned char addr) {

    // ensure the two lines are set for input (we are a slave)
#ifdef __USE18F26J50
    PORTBbits.SCL1 = 1;
    PORTBbits.SDA1 = 1;
#else
    TRISCbits.TRISC3 = 1;
    TRISCbits.TRISC4 = 1;
#endif

    // set the address
    SSPADD = addr;
    //OpenI2C(SLAVE_7,SLEW_OFF); // replaced w/ code below
    SSPSTAT = 0x0;
    SSPCON1 = 0x0;
    SSPCON2 = 0x0;
    SSPCON1 |= 0x0E; // enable Slave 7-bit w/ start/stop interrupts
    SSPSTAT |= SLEW_OFF;
#ifdef I2C_V3
    I2C1_SCL = 1;
    I2C1_SDA = 1;
#else 
#ifdef I2C_V1
    I2C_SCL = 1;
    I2C_SDA = 1;
#else
#ifdef __USE18F26J50
    PORTBbits.SCL1 = 1;
    PORTBbits.SDA1 = 1;
#else
    __dummyXY=35;// Something is messed up with the #ifdefs; this line is designed to invoke a compiler error
#endif
#endif
#endif
    // enable clock-stretching
    SSPCON2bits.SEN = 1;
    SSPCON1 |= SSPENB;
    // end of i2c configure
}